#include <iostream>
#include <string>

#include "Modeler.h"

using namespace std;
using namespace elisa;

/**
 * Planer two link manipulator.
 * [Esposito, HSCC2001]
 */

template<typename Var> 
void PlanerCont(int n, Var *dv, const Var *v, Var t, void *param)
{
	// theta1
	dv[0] = 0.0;
	// theta2
	dv[1] = 0.2;
}

class Planer : public Modeler
{
private:
	static const int n;

	Time t;
	ContVar theta1;
	ContVar theta2;
	ContVarVec cvv;

	ADPtr ad;
	RealConstraint ac;
	ContConstraint m_cc;

public:
	static const RealConstant l1;
	static const RealConstant l2;

	Planer()
		: ad(new vnodelp::FADBAD_AD(n, PlanerCont, PlanerCont)),
		  theta1(2.0, "theta1"),
		  theta2(3.0, "theta2"),
		  t(interval(0, 100)),
		  m_cc(new ContConstraintRep(ad, cvv, t)),

		  ac(sqrt(pow(l1*cos(theta1) + l2*cos(theta1 + theta2), 2) 
				+ pow(l1*cos(theta1) + l2*cos(theta1 + theta2), 2)) == l1 + l2)
	{
		t.setPrecision(1.0e-9);

		//t.setDomain(new Interval(inf(nextT), 100.0));

		cvv.push_back(&theta1);
		cvv.push_back(&theta2);
		t.addInitValue(cvv);

		ContVar::logIndex = 2;
	}

	elisa::Model *model()
	{
		return new Model(m_cc && ac);
	}

	elisa::ContConstraint & cc() 
	{
		return m_cc;
	}

	bool reset()
	{
		return true;
	}
	
	void print()
	{
		cout << '\t' << t.getName() << " = " << t.domain() << endl;
		cout << '\t' << "theta1" << " = " << theta1.domain() << endl;
		cout << '\t' << "theta2" << " = " << theta2.domain() << endl;
	}

	/*
	RealVar & varX()
	{
		return t.domain().getTime();
	}
	RealVar & varY()
	{
		return y;
	}
	*/
};

const int Planer::n(2);
const RealConstant Planer::l1(8.0);
const RealConstant Planer::l2(6.0);
